#ifndef CARTOGRAPHER_SUBMAP_UPDATE_H_
#define CARTOGRAPHER_SUBMAP_UPDATE_H_

#include "cartographer/rcm/time.h"
#include "cartographer/rcm/common.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/range_data_inserter_interface.h"

namespace cartographer
{
    struct ClosureId
    {
        ClosureId()
            : submap_id(0, 0)
            , node_id(0, 0)
        {
        }

        ClosureId(const ClosureId &id)
            : submap_id(id.submap_id.trajectory_id, id.submap_id.submap_index)
            , node_id(id.node_id.trajectory_id, id.node_id.node_index)
        {
        }

        ClosureId(mapping::SubmapId psubmap_id, mapping::NodeId pnode_id)
            : submap_id(psubmap_id.trajectory_id, psubmap_id.submap_index)
            , node_id(pnode_id.trajectory_id, pnode_id.node_index)
        {
        }

        mapping::SubmapId submap_id;
        mapping::NodeId node_id;

        bool operator==(const ClosureId &other) const
        {
            auto pair1 = std::forward_as_tuple(submap_id.trajectory_id, submap_id.submap_index, node_id.trajectory_id, node_id.node_index);
            auto pair2 = std::forward_as_tuple(other.submap_id.trajectory_id, other.submap_id.submap_index, other.node_id.trajectory_id,
                                               other.node_id.node_index);
            return pair1 == pair2;
        }

        bool operator!=(const ClosureId &other) const
        {
            return !operator==(other);
        }

        bool operator<(const ClosureId &other) const
        {
            auto pair1 = std::forward_as_tuple(submap_id.trajectory_id, submap_id.submap_index, node_id.trajectory_id, node_id.node_index);
            auto pair2 = std::forward_as_tuple(other.submap_id.trajectory_id, other.submap_id.submap_index, other.node_id.trajectory_id,
                                               other.node_id.node_index);

            return pair1 < pair2;
        }
    };

    struct ClosureInfo
    {
        // ClosureInfo(const ClosureInfo &o)
        //     : closure_id(o.closure_id)
        //     , relativePose(o.relativePose)
        //     , time(o.time)
        //     , score(o.score)
        // {
        // }

        ClosureId closure_id;
        mapping::PoseGraphInterface::Constraint::Pose relativePose;
        std::chrono::steady_clock::time_point time;
        float score;
    };

    inline void Print(const char *prefix, const ClosureId &cid)
    {
        int nodeIdx = cid.node_id.node_index;
        int nodeTrajId = cid.node_id.trajectory_id;
        int mapIdx = cid.submap_id.submap_index;
        int mapTrajId = cid.submap_id.trajectory_id;
        LOG(INFO) << "[" << prefix << "] submap(" << mapTrajId << "," << mapIdx << ") node(" << nodeTrajId << ", " << nodeIdx << ") ";
    }

    inline void Print(const char *prefix, const ClosureInfo &ci)
    {
        const auto &cid = ci.closure_id;
        int nodeIdx = cid.node_id.node_index;
        int nodeTrajId = cid.node_id.trajectory_id;
        int mapIdx = cid.submap_id.submap_index;
        int mapTrajId = cid.submap_id.trajectory_id;
        LOG(INFO) << "[" << prefix << "] submap(" << mapTrajId << "," << mapIdx << ") node(" << nodeTrajId << ", " << nodeIdx << ") score "
                  << ci.score;
    }

    class SubmapUpdator2d
    {
    public:
        using Constraint = mapping::PoseGraphInterface::Constraint;

        SubmapUpdator2d();

        void AddRangeDataInserter(int trajectory_id, const mapping::proto::SubmapsOptions2D &options);

        void UpdateClosureRelations(const std::vector<Constraint> &result);

        bool UpdateClosureRelations(const Constraint &cost);

        std::vector<ClosureInfo> GetNeedUpdateSubmaps();

        bool InsertRange(mapping::Submap2D *submap, const ClosureId &closure_id, const sensor::RangeData &cloudLocal2d);

    private:
        bool CheckClosureRelation(const mapping::SubmapId &submap_id, const mapping::NodeId &node_id);

        std::map<mapping::NodeId, bool> updateFlags_;
        // 一个节点会与多个submap产生回环，记录其分数
        std::map<mapping::NodeId, ClosureInfo> closureMapping_;
        // std::map<mapping::NodeId, std::vector<ClosureInfo>> closureRelations_;
        std::map<int, std::shared_ptr<mapping::RangeDataInserterInterface>> range_inserters_;
    };
} // namespace cartographer

#endif